/*
 * PI.h
 *
 *  Created on: 2024年11月17日
 *      Author: zizhong
 */

#ifndef CONTROLLIB_PI_H_
#define CONTROLLIB_PI_H_

#define     wcc         300.0  //hz
#define     Rs          0.28
#define     Ls          0.0002
#define     Kp          (Ls * 6.28 * wcc)
#define     Ki          (Rs * 6.28 * wcc * 0.0001)
#define     Kerr        (0.5 * Ki + Kp)
#define     KerrOld     (0.5 * Ki - Kp)
#define     BemfK       0.2   // V/Hz

#define    kp_spd   0.003
#define    ki_spd   0.02
#define    spd_MAX      2.5
#define    spd_MIN      -2.5

typedef struct FCL_cmplxCtrl_t
{
    float  ref;             // Input: reference set-point
    float  fbk;             // Input: feedback
    float  err;             // Output : error
    float  out;             // Output: controller output
    float  carryOver;       // Output : carry over for next iteration
//    float  Kp;              // Parameter: proportional loop gain
//    float  Ki;              // Parameter: integral gain
//    float  Kerr;            // Parameter: gain for latest error
//    float  KerrOld;         // Parameter: gain for prev error
    float  Umax;            // Parameter: upper saturation limit
    float  Umin;            // Parameter: lower saturation limit
    float  cosWTs;
    float  sinWTs;
    float  expVal;
    float  kDirect;
    float  xErr;
} FCL_cmplxCtrl_t;

typedef struct cur_loop_t
{
    float d_err;
    float q_err;
    float we;
    float d_out;
    float q_out;

    float carryOut_d;
    float carryOut_q;

    float preWe;
}fcl_cur_loop_t;

typedef struct spd_loop_t
{
    long  spdCnt;
    float spd_err;
    float iq_out;
    float integrator;
    float zone;

}fcl_spd_loop_t;

#define FCL_CMPLXCTRL_DEFAULTS {                                               \
        0.0,        /* ref */                                                  \
        0.0,        /* fbk */                                                  \
        0.0,        /* err */                                                  \
        0.0,        /* out */                                                  \
        0.0,        /* carryOver */                                            \
        1.0,        /* Kp */                                                   \
        0.1,        /* Ki */                                                   \
        0.0,        /* Kerr */                                                 \
        0.0,        /* KerrOld */                                              \
        1.0,        /* Umax */                                                 \
        -1.0,       /* Umin */                                                 \
        0.0,        /* cosWTs */                                               \
        0.0,        /* sinWTs */                                               \
        0.0,        /* expVal */                                               \
        0.0,        /* kDirect */                                              \
        0.0         /* xErr */                                                 \
}

//
// CURRENT PI CONTROLLER ROUTINES
//
//static inline void FCL_runPI(FCL_cmplxCtrl_t *pPI)
//{
//    pPI->out += (pPI->err * pPI->Kerr) + pPI->carryOver;
//    pPI->out = __fsat(pPI->out, pPI->Umax, pPI->Umin);
//
//    return;
//}

static inline void cur_PI(fcl_cur_loop_t *pPI, int lsw)
{

    //ENC_WAIT_FOR_ALIGNMENT
    if(0 == lsw)
    {
        pPI->q_out = 0;
        pPI->carryOut_q = 0;
    }
    else
    {
        pPI->q_out += ((pPI->q_err * Kerr) + pPI->carryOut_q);
        pPI->carryOut_q = pPI->q_err * KerrOld + BemfK * (pPI->we - pPI->preWe);
//        pPI->carryOut_q = pPI->q_err * KerrOld;
    }
    pPI->d_out += ((pPI->d_err * Kerr) + pPI->carryOut_d);
    pPI->carryOut_d = pPI->d_err * KerrOld;
    pPI->preWe = pPI->we;

}

static inline void spd_PI(fcl_spd_loop_t * pPi, int lsw)
{
    pPi->spdCnt++;
    if(pPi->spdCnt >= 10)
    {
        pPi->spdCnt = 0;
        if(0 == lsw)
        {
            pPi->iq_out = 0.0;
        }
        else
        {
            float out = kp_spd * pPi->spd_err + 0.0001f * pPi->integrator;
            if(out > spd_MAX)
            {
                out = spd_MAX;
                pPi->zone -= spd_MAX;
            }
            else if(out < spd_MIN)
            {
                out = spd_MIN;
                pPi->zone -= spd_MIN;
            }
                else
            {
                pPi->zone = 0.0;
            }
            float integratorGain = ki_spd * pPi->spd_err;
            int tmp0 = pPi->zone > 0.0f ? 1 : -1;
            int tmp1 = integratorGain > 0.0f? 1 : -1;
            if((pPi->zone != 0.0f) && (tmp0 == tmp1))
            {
                integratorGain = 0.0;
            }
                pPi->integrator += integratorGain;
                pPi->iq_out = out;
        }
    }
    else
    {
        pPi->iq_out = pPi->iq_out;
    }
}



#endif /* CONTROLLIB_PI_H_ */
